#include "MoteCollection.h"
#include <iostream>
#include <pthread.h>
#include <time.h>

/*!
 * Creates a new empty MoteCollection. Motes can then be added.
 *
*/
MoteCollection::MoteCollection()
{
	lightLevel = 0; //it will be increased automatically if higher levels are detected
	darkLevel = 10; //it will be lowered automatically if lower levels are detected	
}

MoteCollection::~MoteCollection()
{
	for(vector<Mote*>::iterator it = motes.begin(); it != motes.end(); ++it){
		delete *it;
	}
}

/*!
 * Counts the amount of motes currently in the collection.
 * \return 	The amount of motes in the current collection
*/
int MoteCollection::Count() const
{
	return motes.size();
}

/*!
 * Returns the position of the mote with the specified Id. If
 * the mote does not exist, it returns -1.
 * @param[in]	moteId	The requested moteId to find
 * @return	The position of the mote if it is found or -1 if the mote is not found.
*/
int MoteCollection::MotePosition(int moteId) const{
	int i = 0;
	for(vector<Mote*>::const_iterator it = motes.begin(); it != motes.end(); ++it){
		if((**it).GetId() == moteId){
			return i;
		}
		++i;
	}
	return -1;
}

/*!
 * Determines if the mote currently exists in the collection.
 * @param[in] moteId	The id of the mote
 * @return True if the mote is in the collection, else false
 */
bool MoteCollection::MoteExists(int moteId) const{
	return (MotePosition(moteId) != -1);
}

/*!
 * Changes the light sensor reading for a given mote.
 * \param[in] newReading	The new reading for the mote
 * \param[in] moteId		The moteId that specifies which mote the value should be changed
 * \return	True if the mote reading is changed. False if the mote is not found.
*/
bool MoteCollection::changeReading(int newReading, int moteId)
{
	if(newReading > lightLevel)
	{
		lightLevel = newReading;
	}

	if(newReading < darkLevel)
	{
		darkLevel = newReading;
	}

	DEBUG("Mote: " << moteId << ", set reading: " << newReading);
	int pos = MotePosition(moteId);
	if(pos != -1){
		motes[pos]->SetReading(newReading);
		return SUCCESS;
	}
	else {
		return FAILURE;
	}
}

/*!
 * Gets the highest level of light found
 * \return	The value of highest level of light found
 */
int MoteCollection::LightLevel()
{
	return lightLevel;
}

/*!
 * Gets the lowest level of light found
 * \return	The value of lowest level of light found
 */
int MoteCollection::DarkLevel()
{
	return darkLevel;
}

/*!
 * Resets the highest and lowest level of lights 
 */
void MoteCollection::ResetLevels()
{
	lightLevel = 0;
	darkLevel = 10;
}

/*!
 * Returns a copy of the list of motes
 * \return		The list of motes
*/	
const vector<Mote*> MoteCollection::Motes() const
{
	return motes;
}

 /*!
 * Adds a new mote to the collection if possible. Does not
 * add a mote if one already exists with the same ID.
 * \param[in] moteId	The ID of the mote to be added
 * \return	True if the mote is added successfully. False if there 
 * 			already exists a mote with the same ID or if MAX_MOTE_COUNT is achieved. 
 */
bool MoteCollection::addMote(int moteId)
{
	if(Count() >= MAX_MOTE_COUNT || MoteExists(moteId)){
		return FAILURE;
	}
	DEBUG("Discovered Mote: " << moteId << ", Adding");
	motes.push_back(new Mote(moteId));
	return SUCCESS;
}

/*!
 * Adds a new mote to the collection if it is unique.
 * \param[in] m	The new mote to be added
 * \return	True if the mote is successfully added. False if the mote already exists or if MAX_MOTE_COUNT is achieved.
 */
bool MoteCollection::addMote(Mote* m)
{
	if(Count() >= MAX_MOTE_COUNT || MoteExists(m->GetId())){
		return FAILURE;
	}
	motes.push_back(m);
	return SUCCESS;
}

/*!
 * Finds the first mote with the given ID, if one exists, else 
 * returns null.
 * \param[in]	moteId	The ID of the mote to find
 * \return	The mote from the given ID if found, else returns null
 */
Mote* MoteCollection::MoteById(int moteId)
{
	for(vector<Mote*>::iterator it = motes.begin(); it != motes.end(); ++it){
		if((**it).GetId() == moteId){
			return *it;
		}
	}
	return NULL;
}


/*!
 * Defines the behaviour of outputing a MoteCollection to an output stream
 * \param[in]	out	The output stream (usually cout)
 * \param[in]	c	The MoteCollection variable
 * \return	A print out of Motes in MoteCollection c
 */
ostream& operator<<(ostream& out, const MoteCollection& c){
	if(c.Count() > 0){
		vector<Mote*> motes = c.Motes();
		for(vector<Mote*>::const_iterator it = motes.begin(); it != motes.end(); ++it){
			out << **it << " ";
		}
	}
	else{
		out << "No motes detected";
	}
	return out;	
}
	
/*!
 * Sets the LEDs of the given mote. (To be implemented in the future)
 * \param moteId	The Id of the mote to set
 * \param LEDs		The LEDs of the mote as a short. 
 * <br>	1 for LED 0.
 * <br>	2 for LED 1.
 * <br> 4 for LED 2.
 * <br>For example, 7 is all on, and 0 is all off. 
 * \return	If the packet was successfully queued to be transfered to the
 * GameHub to be relayed to the mote. Will fail if the mote does
 * not exist or the serial write buffer is full.
 */
bool MoteCollection::setLEDs(int moteId, short LEDs){
	//to be implemented in the future
}

/*!
 * A main method for testing readings.
 * \param[in] argc
 * \param[in] argv
 */
int oldmain(int argc, char** argv)
{	
	MoteCollection* motes = new MoteCollection();	

	MoteBridge bridge(motes);
	
	struct timespec ts;
	ts.tv_sec = 2;
	ts.tv_nsec = 0;

	while(true){
		cout << (*motes) << endl;
		nanosleep (&ts, NULL);
	}
	
	delete motes;
	
	return 0;
}
